﻿using System;
using System.Collections.Generic;
using System.Text;
using RoBOSSCommunicator;

namespace MazeRaider
{
    class Robot
    {
        private const double MAX_FORCE = 40.0;
        private const double DESIRED_VELOCITY = 3.0;

        private String mazeRaiderId;
        private int rawRobotId;
        private RoBOSSCommunicator.Robot rawRobot;
        private Communicator communicator = new Communicator();

        private double? northRotation = null;
        private bool stopFlag = false;

        public Robot(String mazeRaiderId, int rawRobotId)
        {
            this.mazeRaiderId = mazeRaiderId;
            this.rawRobotId = rawRobotId;
        }

        public void Request()
        {
            int result = communicator.Connect("192.168.0.11", "4468", mazeRaiderId);
            if (result == 0)
            {
            }
            else
            {
                Console.WriteLine("Error while connecting to controller");
            }

            int successful = communicator.RequestRobot(rawRobotId);
            if (successful == 0)
            {
                Console.WriteLine(mazeRaiderId + " successfully requested (1 of "
                    + communicator.ownedRobotsCount + ") raw robot of raw id \"" + rawRobotId);
                rawRobot = communicator.ownedRobots[0];
            }
            else
            {
                Console.WriteLine("Error while requesting " + mazeRaiderId + " raw robot");
            }
        }

        public void Release()
        {
            int successful = communicator.ReleaseRobot(rawRobotId);
            if (successful == 0)
            {
                Console.WriteLine(mazeRaiderId + " successfully released" + " raw robot of raw id \""
                    + rawRobotId + "\"");
                rawRobot = null;
            }
            else
            {
                Console.WriteLine("Error while releasing " + mazeRaiderId + " raw robot");
            }
            communicator.Dispose();
        }

        public double? NorthRotation
        {
            get
            {
                return northRotation;
            }
            set
            {
                northRotation = value;
            }
        }

        public bool StopFlag
        {
            get
            {
                return stopFlag;
            }
            set
            {
                stopFlag = value;
            }
        }

        public unsafe RobotInfo GetRobotInfo()
        {
            communicator.Receive(Communicator.RECEIVEBLOCKLEVEL_WaitForTimestamp);
            double* pos = rawRobot.position;
            double* rot = rawRobot.rotation;
            if (northRotation == null)
            {
                northRotation = rot[0];
            }
            RobotInfo position = new RobotInfo(pos[0], pos[1], rot[0]);
            return position;
        }

        public void OrderTurnMoveOrStop(double firstJointSign, double secondJointSign)
        {
            communicator.Receive(Communicator.RECEIVEBLOCKLEVEL_WaitForNewTimestamp);
            rawRobot.joints[0].motorMaxForce = firstJointSign * MAX_FORCE;
            rawRobot.joints[1].motorMaxForce = secondJointSign * MAX_FORCE;
            rawRobot.joints[0].motorDesiredVelocity = firstJointSign * DESIRED_VELOCITY;
            rawRobot.joints[1].motorDesiredVelocity = secondJointSign * DESIRED_VELOCITY;
            communicator.Send();
        }

        public void OrderMoveForward()
        {
            OrderTurnMoveOrStop(1.0, 1.0);
        }

        public void OrderMoveBackward()
        {
            OrderTurnMoveOrStop(-1.0, -1.0);
        }

        public void OrderTurnLeft()
        {
            OrderTurnMoveOrStop(-1.0, 1.0);
        }

        public void OrderTurnRight()
        {
            OrderTurnMoveOrStop(1.0, -1.0);
        }

        public void OrderStop()
        {
            OrderTurnMoveOrStop(0.0, 0.0);
        }

        public void TurnNorth()
        {
            RobotInfo info = GetRobotInfo();
            if (MathUtil.AreAbsolutesEqual(info.Rot, (double) northRotation))
            {
                return;
            }
            OrderTurnRight();
            while (!MathUtil.AreAbsolutesEqual(info.Rot, (double) northRotation))
            {
                if (stopFlag)
                {
                    break;
                }
                info = GetRobotInfo();
            }
            OrderStop();
        }

        public void TurnSouth()
        {
            RobotInfo info = GetRobotInfo();
            if (MathUtil.AreAbsolutesEqual(info.Rot, 1.0 - Math.Abs((double) northRotation)))
            {
                return;
            }
            OrderTurnRight();
            while (!MathUtil.AreAbsolutesEqual(info.Rot, 1.0 - Math.Abs((double) northRotation)))
            {
                if (stopFlag)
                {
                    break;
                }
                info = GetRobotInfo();
            }
            OrderStop();
        }

        public void TurnWest()
        {
            TurnNorth();
            RobotInfo info = GetRobotInfo();
            if (MathUtil.AreAbsolutesEqual(info.Rot, 0.66 - Math.Abs((double)northRotation)))
            {
                return;
            }
            OrderTurnLeft();
            while (!MathUtil.AreAbsolutesEqual(info.Rot, 0.66 - Math.Abs((double)northRotation)))
            {
                if (stopFlag)
                {
                    break;
                }
                info = GetRobotInfo();
            }
            OrderStop();
        }

        public void TurnEast()
        {
            TurnNorth();
            RobotInfo info = GetRobotInfo();
            if (MathUtil.AreAbsolutesEqual(info.Rot, 0.66 - Math.Abs((double)northRotation)))
            {
                return;
            }
            OrderTurnRight();
            while (!MathUtil.AreAbsolutesEqual(info.Rot, 0.66 - Math.Abs((double)northRotation)))
            {
                if (stopFlag)
                {
                    break;
                }
                info = GetRobotInfo();
            }
            OrderStop();
        }

        public void MoveDistance(double distance)
        {
            RobotInfo start = GetRobotInfo();
            OrderMoveForward();
            RobotInfo current = GetRobotInfo();
            while (MathUtil.CalculateDistance(start, current) < distance)
            {
                if (stopFlag)
                {
                    break;
                }
                current = GetRobotInfo();
            }
            OrderStop();
        }

        public void MoveRobotToNearestRoundedCoordinates()
        {
            RobotInfo info = GetRobotInfo();
            double desiredX = Math.Round(info.X, 1);
            double desiredY = Math.Round(info.Y, 1);

            bool forward = info.X < desiredX;
            bool right = info.Y < desiredY;

            if (forward)
            {
                TurnNorth();
            }
            else
            {
                TurnSouth();
            }
            MoveDistance(Math.Abs(info.X - desiredX));

            if (right)
            {
                TurnEast();
            }
            else
            {
                TurnWest();
            }
            MoveDistance(Math.Abs(info.Y - desiredY));
        }

        public void MoveNorth()
        {
            RobotInfo info;

            info = GetRobotInfo();
            double desiredX = info.X + 0.1;
            double desiredY = info.Y;

            MoveRobotToNearestRoundedCoordinates();
            TurnNorth();
            MoveDistance(0.1);

            info = GetRobotInfo();
            double actualX = info.X;
            double actualY = info.Y;

            Console.WriteLine("Move north error [" + Math.Abs(desiredX - actualX) + ", "
                + Math.Abs(desiredY - actualY) + "]");
            Console.WriteLine("Current position [" + info.X + ", " + info.Y + "]");
        }

        public void MoveSouth()
        {
            RobotInfo info;

            info = GetRobotInfo();
            double desiredX = info.X - 0.1;
            double desiredY = info.Y;

            MoveRobotToNearestRoundedCoordinates();
            TurnSouth();
            MoveDistance(0.1);

            info = GetRobotInfo();
            double actualX = info.X;
            double actualY = info.Y;

            Console.WriteLine("Move south error [" + Math.Abs(desiredX - actualX) + ", "
                + Math.Abs(desiredY - actualY) + "]");
            Console.WriteLine("Current position [" + info.X + ", " + info.Y + "]");
        }

        public void MoveWest()
        {
            RobotInfo info;

            info = GetRobotInfo();
            double desiredX = info.X;
            double desiredY = info.Y - 0.1;

            MoveRobotToNearestRoundedCoordinates();
            TurnWest();
            MoveDistance(0.1);

            info = GetRobotInfo();
            double actualX = info.X;
            double actualY = info.Y;

            Console.WriteLine("Move west error [" + Math.Abs(desiredX - actualX) + ", "
                + Math.Abs(desiredY - actualY) + "]");
            Console.WriteLine("Current position [" + info.X + ", " + info.Y + "]");
        }

        public void MoveEast()
        {
            RobotInfo info;

            info = GetRobotInfo();
            double desiredX = info.X;
            double desiredY = info.Y + 0.1;

            MoveRobotToNearestRoundedCoordinates();
            TurnEast();
            MoveDistance(0.1);

            info = GetRobotInfo();
            double actualX = info.X;
            double actualY = info.Y;

            Console.WriteLine("Move east error [" + Math.Abs(desiredX - actualX) + ", "
                + Math.Abs(desiredY - actualY) + "]");
            Console.WriteLine("Current position [" + info.X + ", " + info.Y + "]");
        }
    }
}
